#include "daq.h"
#include "util/util.h"
#include <stdio.h>

using namespace DAQ_namespace;

extern DAQALLOC DAQ;

extern EVENT_HANDLE eh;
extern DWORD eventMsk;
extern DWORD eventWord;

extern pthread_t primary;  //this is the primary thread.  The program state machine exists here
extern pthread_t receive;  //this thread handles and buffers incoming Rx data







DAQALLOC::DAQALLOC()
{

//Analog In

    this->analogIn[0].settingPending    = false;
    this->analogIn[0].enable            = false;
    this->analogIn[0].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[0].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[0].resolution        = ADC_14_BIT_gc;
    this->analogIn[0].sign              = true;

    this->analogIn[1].settingPending    = false;
    this->analogIn[1].enable            = false;
    this->analogIn[1].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[1].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[1].resolution        = ADC_14_BIT_gc;
    this->analogIn[1].sign              = true;

    this->analogIn[2].settingPending    = false;
    this->analogIn[2].enable            = false;
    this->analogIn[2].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[2].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[2].resolution        = ADC_14_BIT_gc;
    this->analogIn[2].sign              = true;

    this->analogIn[3].settingPending    = false;
    this->analogIn[3].enable            = false;
    this->analogIn[3].gain              = ADC_CH_GAIN_1X_gc;
    this->analogIn[3].samples           = ADC_16_SAMPLES_gc;
    this->analogIn[3].resolution        = ADC_14_BIT_gc;
    this->analogIn[3].sign              = true;

//Analog Out

    this->analogOut[0].enable           = false;
    this->analogOut[0].settingPending   = false;

//Digital

    this->digital.inSettingPending      = false;
    this->digital.outSettingPending     = false;
    this->digital.inEnable              = false;
    this->digital.outEnable             = false;
    this->digital.dioAlloc[13]          = {0,0,0,0,0,0,0,0,0,0,0,0,0};
    this->digital.dir[13]               = {0,0,0,0,0,0,0,0,0,0,0,0,0};

//Encoder

    this->encoder[0].settingPending     = false;
    this->encoder[0].absolute           = false;
    this->encoder[0].enable             = false;
    this->encoder[0].index              = false;
    this->encoder[0].PLOC               = NULL;
    this->encoder[0].resolution         = 0;

    this->encoder[1].settingPending     = false;
    this->encoder[1].absolute           = false;
    this->encoder[1].enable             = false;
    this->encoder[1].index              = false;
    this->encoder[1].PLOC               = NULL;
    this->encoder[1].resolution         = 0;

//Motor
    this->motor[0].settingPending       = false;
    this->motor[0].enable               = false;
    this->motor[1].settingPending       = false;
    this->motor[1].enable               = false;
    this->motor[2].settingPending       = false;
    this->motor[2].enable               = false;
    this->motor[3].settingPending       = false;
    this->motor[3].enable               = false;

//Servo
    this->servo[0].settingPending       = false;
    this->servo[0].enable               = false;
    this->servo[1].settingPending       = false;
    this->servo[1].enable               = false;
    this->servo[2].settingPending       = false;
    this->servo[2].enable               = false;
    this->servo[3].settingPending       = false;
    this->servo[3].enable               = false;

//General

    this->general.startPending          = false;
    this->general.stopPending           = false;
    this->general.actuatorSuperPending  = false;
    this->general.outputSuperPending    = false;

//Timer

    this->timer.enable                  = false;
    this->timer.loopTime                = 0;
    this->timer.settingPending          = false;

//State machine
    this->state                         = DAQ_INIT_STATE;
}

status_t DAQALLOC::sendAdcInit(FT_HANDLE handle,uint8_t adcMsk, GAIN_t adcGain)
{
    adcInit_t packet;

    packet.Header         = packetHeader;
    packet.msgHeader      = adcInit_mh;
    packet.PARAM1.ACHM    = adcMsk;
    packet.PARAM1.GAIN    = adcGain;

    //set pending flags for every affected channel
    if(adcMsk & ADC_CH0_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[0].settingPending = true;
        this->analogIn[0].gain = adcGain;
    }
    if(adcMsk & ADC_CH1_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[1].settingPending = true;
        this->analogIn[1].gain = adcGain;
    }
    if(adcMsk & ADC_CH2_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[2].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[2].settingPending = true;
        this->analogIn[2].gain = adcGain;
    }
    if(adcMsk & ADC_CH3_bp)
    {
        //check to see if a setting is already pending
        if(this->analogIn[3].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogIn[3].settingPending = true;
        this->analogIn[3].gain = adcGain;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(adcInit_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(adcInit_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendDacEnable(FT_HANDLE handle, DACM_t DACM)
{

    dacEnable_t packet;

    packet.Header           = packetHeader;
    packet.msgHeader        = dacEnable_mh;
    packet.PARAM1.DCHM      = DACM;

    if(DACM == DAC_CH0_bp)
    {
        //see if a setting is pending
        if(this->analogOut[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogOut[0].settingPending = true;
    }
    if(DACM == DAC_CH1_bp)
    {
        //see if a setting is pending
        if(this->analogOut[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->analogOut[1].settingPending = true;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(dacEnable_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(dacEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendDigitalInEnable(FT_HANDLE handle)
{
    digitalInEnable_t packet;

    packet.Header     = packetHeader;
    packet.msgHeader  = digitalInEnable_mh;

    //check for a pending setting
    if(this->digital.inSettingPending)
    {
        return OPP_FAILED;
    }

    this->digital.inSettingPending = true;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(digitalInEnable_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(digitalInEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendDigitalOutEnable(FT_HANDLE handle)
{
    digitalOutEnable_t packet;

    packet.Header     = packetHeader;
    packet.msgHeader  = digitalOutEnable_mh;

    if(this->digital.outSettingPending)
    {
        return OPP_FAILED;
    }
    this->digital.outSettingPending = true;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(digitalOutEnable_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(digitalOutEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendEncoderInit(FT_HANDLE handle, PLOC_t PLOC, bool index, uint8_t num, bool ABS, DIGFILT_t EDFL, uint16_t resolution)
{
    encInit_t packet;

    WORD_t res;

    res.word = resolution;

    packet.Header             = packetHeader;
    packet.msgHeader          = encInit_mh;

    packet.PARAM1.ENCN        = num;
    packet.PARAM1.INDX        = index;
    packet.PARAM1.PLOC        = PLOC;

    packet.PARAM2.ABS         = ABS;
    packet.PARAM2.EDFL        = EDFL;
    packet.PARAM2.ERESH       = res.bytes.byteH & 0x0F;
    packet.PARAM3.ERESL       = res.bytes.byteL;

    if(num == 0)
    {
        if(this->encoder[0].settingPending)
        {
            return OPP_FAILED;
        }

        this->encoder[0].settingPending = true;
        this->encoder[0].absolute = ABS;
        this->encoder[0].index = index;
        this->encoder[0].PLOC = PLOC;
        this->encoder[0].resolution = resolution;
    }
    if(num == 1)
    {
        if(this->encoder[1].settingPending)
        {
            return OPP_FAILED;
        }

        this->encoder[1].settingPending = true;
        this->encoder[1].absolute = ABS;
        this->encoder[1].index = index;
        this->encoder[1].PLOC = PLOC;
        this->encoder[1].resolution = resolution;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(encInit_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(encInit_mh));

    return OPP_SUCCESSFUL;

}

status_t DAQALLOC::sendMotorEnable(FT_HANDLE handle, uint8_t msk)
{
    motorEnable_t packet;

    packet.Header                 = packetHeader;
    packet.msgHeader              = motorEnable_mh;

    packet.PARAM1.motorEn_msk     = msk & 0xF;

    if(msk & 0x01)
    {
        if(this->motor[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[0].settingPending = true;
    }
    if(msk & 0x02)
    {
        if(this->motor[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[1].settingPending = true;
    }
    if(msk & 0x04)
    {
        if(this->motor[2].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[2].settingPending = true;
    }
    if(msk & 0x08)
    {
        if(this->motor[3].settingPending)
        {
            return OPP_FAILED;
        }
        this->motor[3].settingPending = true;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(motorEnable_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(motorEnable_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendServoEnable(FT_HANDLE handle, uint8_t msk)
{
    servoEnable_t packet;

    packet.Header                 = packetHeader;
    packet.msgHeader              = servoEnable_mh;

    packet.PARAM1.servoEn_msk     = msk & 0xF;

    if(msk & 0x01)
    {
        if(this->servo[0].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[0].settingPending = true;
    }
    if(msk & 0x02)
    {
        if(this->servo[1].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[1].settingPending = true;
    }
    if(msk & 0x04)
    {
        if(this->servo[2].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[2].settingPending = true;
    }
    if(msk & 0x08)
    {
        if(this->servo[3].settingPending)
        {
            return OPP_FAILED;
        }
        this->servo[3].settingPending = true;
    }

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(servoEnable_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(servoEnable_mh));
}

status_t DAQALLOC::sendSetTimerFreq(FT_HANDLE handle,uint16_t freq)
{
    tmrSetFreq_t packet;

    packet.Header       = packetHeader;
    packet.msgHeader    = tmrSetFreq_mh;

    packet.frequency    = freq;

    if(this->timer.settingPending)
    {
        return OPP_FAILED;
    }
    this->timer.settingPending = true;
    this->timer.loopTime = freq;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(tmrSetFreq_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(tmrSetFreq_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendStartProgram(FT_HANDLE handle)
{
    programStart_t packet;

    packet.Header       = packetHeader;
    packet.msgHeader    = startProgram_mh;

    if(this->general.startPending)
    {
        return OPP_FAILED;
    }
    this->general.startPending = true;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(startProgram_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(startProgram_mh));

    return OPP_SUCCESSFUL;
}

status_t DAQALLOC::sendStopProgram(FT_HANDLE handle)
{
    programStop_t packet;

    packet.Header       = packetHeader;
    packet.msgHeader    = stopProgram_mh;

    if(this->general.stopPending)
    {
        return OPP_FAILED;
    }

    this->general.stopPending = true;

    crcAppend((uint8_t *)&packet.Header,packetSizeLookup(stopProgram_mh));
    comPortPut(handle,(uint8_t *)&packet.Header,packetSizeLookup(stopProgram_mh));

    return OPP_SUCCESSFUL;
}

//Always pass full 4 value deep arrays, un-allocated actuator values will be disregarded
status_t DAQALLOC::sendActuatorSuperPacket(FT_HANDLE handle, uint16_t servoPosition[], uint16_t motorEffort[])
{
    uint8_t packet[this->getActuatorSuperPacketLength()];

    WORD_t val;

    packet[0] = packetHeader;
    packet[1] = actuatorSuperPacket_mh;

    uint8_t pos = 2,inc;

    if(this->general.actuatorSuperPending)
    {
        return OPP_FAILED;
    }

    //SERVOS
    for(inc=0;inc<4;inc++)
    {
        if(this->servo[inc].enable)
        {
            val.word = servoPosition[inc];
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;
        }
    }

    //MOTORS
    for(inc=0;inc<4;inc++)
    {
        if(this->motor[inc].enable)
        {
            val.word = motorEffort[inc];
            packet[pos++] = val.bytes.byteH;
            packet[pos++] = val.bytes.byteL;
        }
    }

    crcAppend(&packet[0],getActuatorSuperPacketLength());
    comPortPut(handle,&packet[0],getActuatorSuperPacketLength());

    return OPP_SUCCESSFUL;

}


